% x = -10:0.1:10;
% y = 1 ./ (1 + exp(-x));
% 
% % 绘制图像
% figure;
% plot(x, y, 'LineWidth', 2);
% hold on;
% plot(-6.5, 1 / (1 + exp(6.5)), 'ro'); % 标记x=-6.5处的值
% grid on;
% xlabel('x');
% ylabel('Sigmoid(x)');
% title('Sigmoid Function');

% MATLAB script to calculate theta such that tan(theta) = 2^(-i)

% MATLAB脚本，计算tan(theta) = 2^-i对应的theta角度，并生成查表数据
% 该脚本适用于CORDIC算法的角度生成
function test()
    % 调用生成角度查表的函数
    [theta_radians, theta_degrees] = generate_angle_table(20);

    % 显示生成的查表
    fprintf('\n生成的角度查表（tan(theta) = 2^-i）：\n');
    for i = 1:length(theta_radians)
        fprintf('i = %d, theta = %.10f 弧度, %.10f 度\n', i-1, round(theta_radians(i)*2^28), theta_degrees(i));
    end

    % 初始化参数
    K = 0.607252935008881 * 2^28;
    K = K / 2^(28-16);
    fprintf('K= %.6f\n',K);
    x = round(K); 
    y = 0; 
    alpha_angle = 20580;
    theta = alpha_angle;
    num_rotations = 20;

    PI_Q28 = 843314856;
    PI = PI_Q28 / 2^(28-16);
    ANGLE_90 = PI / 2;
    ANGLE_180 = PI;
    ANGLE_270 = PI*3/2;
    ANGLE_360 = PI*2;
    if( theta > ANGLE_90 && ANGLE_180 >= theta )
        theta = theta - ANGLE_90;
    end
    if( theta > ANGLE_180 && ANGLE_270 >= theta )
        theta = theta - ANGLE_180;
        
    end
    if( theta > ANGLE_270 && ANGLE_360 >= theta )
        theta = theta - ANGLE_270;
    end
    fprintf('theta %.6f\n',theta);
    % 存储每次旋转的结果
    results = zeros(num_rotations, 3); % 存储 x_buf, y_buf, theta_buf

    for cnt = 1:num_rotations
        [x, y, theta] = cordic_sin_cos(x, y, theta, cnt);
        results(cnt, :) = [x, y, theta];
        fprintf('Rotation %d: x = %.6f, y = %.6f, theta = %.6f\n', cnt, x, y, theta);
    end

    
    fprintf('sin(alpha_angle/2^16) = %.6f,cos(alpha_angle/2^16) = %.6f\n', sin( alpha_angle/2^16 ),cos( alpha_angle/2^16 ) );

    q3 = 100 * 2^16;
    q3 = q3/10;
    fprintf('辅助测试 %.6f\n',q3/2^16);
end

function [theta_radians, theta_degrees] = generate_angle_table(max_i)
    % 初始化数组，用于存储计算的theta值
    theta_radians = zeros(1, max_i);
    theta_degrees = zeros(1, max_i);

    % 计算每个i对应的theta值
    for i = 0:max_i-1
        % 计算tan(theta) = 2^(-i)
        tan_theta = 2^(-i);

        % 求出theta值，使用反正切函数atan
        theta = atan(tan_theta);

        % 存储theta的弧度值和角度值
        theta_radians(i+1) = theta;
        theta_degrees(i+1) = rad2deg(theta);

        % 显示每个i对应的theta值
        fprintf('i = %d: theta = %.10f 弧度, %.10f 度\n', i, theta_radians(i+1), theta_degrees(i+1));
    end
end

function [x_buf, y_buf, theta_buf] = cordic_sin_cos(x, y, theta, cnt)
    ANGLE_Q28 = [210828714; 124459457; 65760959; 33381290; 16755422; 8385879; 4193963; 2097109; 1048571; 524287; 262144; 131072; 65536; 32768; 16384; 8192; 4096; 2048; 1024; 512];
    
    if theta >= 0.0
        % 动态调整移位值为 2^cnt
        x_buf = x - y/2^(cnt-1);
        y_buf = y + x/2^(cnt-1);
        % 更新角度时也根据 cnt 进行位移
        theta_buf = theta - ANGLE_Q28(cnt) / 2^(28 - 16);
    else
        x_buf = x + y/2^(cnt-1);
        y_buf = y - x/2^(cnt-1);
        theta_buf = theta + ANGLE_Q28(cnt) / 2^(28 - 16);
    end
end






% 增量式PID参数初始化
Kp = 1.0; % 比例增益
Ki = 0.1; % 积分增益
Kd = 0.05; % 微分增益

% PID 控制器初始值
prev_error = 0;
prev_error2 = 0;
delta_u = 0; % 增量输出

% 系统初始值
u = 0; % 控制信号
y = 0; % 系统输出
setpoint = 1; % 目标设定值

% 模拟时间设置
dt = 0.1; % 时间步长
t = 0:dt:10000; % 模拟时间
y_history = zeros(size(t)); % 输出记录

for i = 2:length(t)
    % 计算当前误差
    error = setpoint - y;
    
    % 增量式PID控制算法
    delta_u = Kp * (error - prev_error) + Ki * error * dt + Kd * (error - 2 * prev_error + prev_error2) / dt;
    u = u + delta_u; % 更新控制信号
    
    % 使用负载函数计算系统响应
    y = load_function(u); % 模拟负载的输出
    
    % 更新误差
    prev_error2 = prev_error;
    prev_error = error;
    
    % 记录输出
    y_history(i) = y;
end

% 绘制结果
figure;
plot(t, y_history, 'LineWidth', 1.5);
hold on;
plot(t, setpoint * ones(size(t)), '--r'); % 目标设定值
xlabel('Time (s)');
ylabel('Output');
title('Incremental PID Control with Load Function');
legend('System Output', 'Setpoint');
grid on;
function y = load_function(u)
    % 一阶系统：y = (a*y_previous + b*u)/(1 + a)
    % 可以理解为一阶惯性系统

    persistent y_previous
    if isempty(y_previous)
        y_previous = 0;
    end
    
    a = 0.9; % 系统惯性系数，越接近1惯性越大
    b = 0.1; % 控制信号系数
    
    % 计算输出
    y = (a * y_previous + b * u) / (1 + a);
    y_previous = y; % 更新上一时刻输出
end

